#include <eigen_conversions/eigen_msg.h>
#include <ros/ros.h>
#include <pcl/common/transforms.h>
#include <pcl/io/ply_io.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/point_cloud.h>
#include <tf2_ros/transform_listener.h>

#include "ground_segmentation/ground_segmentation.h"
#include "pcl/filters/filter.h"
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/filters/crop_box.h>
#include <Depth/DrivableSeg.h>
#include "glog/logging.h"
#include "tools/file_manager.hpp"

#include <iostream>           //标准C++库中的输入输出类相关头文件。
#include <pcl/io/pcd_io.h>   //pcd 读写类相关的头文件。
#include <pcl/point_types.h> //PCL中支持的点类型头文件

pcl::PointCloud<pcl::PointXYZ>::Ptr fullPointCloud;
pcl::PointCloud<pcl::PointXYZ>::Ptr ground_cloud, noground_cloud,slope_cloud;
pcl::PointCloud<pcl::PointXYZI>::Ptr cluster_cloud,filtered_slope_cloud;

bool zvision_reverse = false;

void copyPointCloud(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg);

double toler=0.05; // angle threshold of two points
int min_size=100;  // The data of KITTI is not the original lidar signal, we need to consider those holes when search for nearest point in four directions.
int max_size=1000000;

class SegmentationNode {
    enum clusterpurpose{
        obstacle = 0,
        plane = 1,
        rmoutlier = 2,
    };
    ros::NodeHandle nh_;

    ros::Publisher ground_pub_;
    ros::Publisher obstacle_pub_;
    ros::Publisher slope_cloud_pub_;
    ros::Publisher reversed_zvision_pub_;
    tf2_ros::Buffer tf_buffer_;
    tf2_ros::TransformListener tf_listener_{tf_buffer_};
    GroundSegmentationParams params_;
    std::string gravity_aligned_frame_;
    pcl::CropBox<pcl::PointXYZ> boundary_box_filter_;
    pcl::CropBox<pcl::PointXYZ> nearby_box_filter_;

    std::shared_ptr<DrivableSeg> dsMethod;

    std_msgs::Header cloudHeader;
public:
    SegmentationNode(ros::NodeHandle &nh,
                     const std::string &ground_topic,
                     const std::string &obstacle_topic,
                     const GroundSegmentationParams &params,
                     const bool &latch = false) : params_(params),nh_(nh) {
        ground_pub_ = nh.advertise<pcl::PointCloud<pcl::PointXYZ>>(ground_topic, 1, latch);
        obstacle_pub_ = nh.advertise<pcl::PointCloud<pcl::PointXYZ>>(obstacle_topic, 1, latch);
        reversed_zvision_pub_ = nh.advertise<pcl::PointCloud<pcl::PointXYZ>>("/reversed_zvision", 1, latch);
        slope_cloud_pub_ = nh.advertise<pcl::PointCloud<pcl::PointXYZ>>("/slope_cloud", 1, latch);

        nh.param<std::string>("gravity_aligned_frame", gravity_aligned_frame_, "");

        float CAR_HEIGHT = 0.408;
        boundary_box_filter_.setMin(Eigen::Vector4f(-20, -20, -CAR_HEIGHT-0.2, 1.0));
        boundary_box_filter_.setMax(Eigen::Vector4f(20, 20, 1.5, 1.0));
        boundary_box_filter_.setNegative(false);//保留框内的

        //滤去没有接收到的点，没有接收到的点都是0，0，0，
        nearby_box_filter_.setMin(Eigen::Vector4f(-0.01, -0.01, -0.01, 1.0));
        nearby_box_filter_.setMax(Eigen::Vector4f(0.01, 0.01, 0.01, 1.0));
        nearby_box_filter_.setNegative(true);//保留框外的

        dsMethod = std::make_shared<DrivableSeg>(nh);
    }

    void scanCallback(const sensor_msgs::PointCloud2ConstPtr laserCloudMsg) {
        GroundSegmentation segmenter(nh_,params_);

        copyPointCloud(laserCloudMsg);

        pcl::PointCloud<pcl::PointXYZ> cloud_transformed;

        std::vector<int> labels;
        //计算平均时间消耗
        static int EnterNumber = 1;
        static double fp_total_ms = 0;
        static double fp_avg_ms = 0;

        segmenter.segment(*fullPointCloud, &labels);

        ground_cloud->clear();
        noground_cloud->clear();
        cluster_cloud->clear();
        slope_cloud->clear();
        filtered_slope_cloud->clear();

        for (size_t i = 0; i < fullPointCloud->size(); ++i) {
            if (labels[i] != 0) {
                ground_cloud->push_back(fullPointCloud->points[i]);
                if(labels[i] == 2)
                    slope_cloud->push_back(fullPointCloud->points[i]);
            }
            else noground_cloud->push_back(fullPointCloud->points[i]);
        }

        std::chrono::high_resolution_clock::time_point start = std::chrono::high_resolution_clock::now();

        bool is_stair_flag = false;//判断斜坡是不是楼梯
        bool temp_flag = false;//这个不起作用，只是为了满足输入要求
        dsMethod->ComputeDrivableArea(slope_cloud,*filtered_slope_cloud,clusterpurpose::plane,500,is_stair_flag);

        dsMethod->ComputeDrivableArea(noground_cloud,*cluster_cloud,clusterpurpose::obstacle,30,temp_flag);

        if(is_stair_flag){
            *cluster_cloud += *filtered_slope_cloud;//如果是楼梯那就把整个点云并入障碍物当中
            filtered_slope_cloud->clear();
        }
        // todo 对no_ground_cloud进行离群点滤波

        //计算耗时
        std::chrono::high_resolution_clock::time_point end = std::chrono::high_resolution_clock::now();
        std::chrono::duration<double, std::milli> fp_ms = end - start;
        fp_total_ms += fp_ms.count();
        fp_avg_ms = fp_total_ms/EnterNumber;
        EnterNumber++;
//        std::cout << "AvgCluster! Took " << fp_avg_ms << "ms\n";
//        std::cout << "TotalCluster! Took " << fp_total_ms << "ms\n";

        PublishCloud();
    }

    void copyPointCloud(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg) {

        cloudHeader = laserCloudMsg->header;
        pcl::fromROSMsg(*laserCloudMsg, *fullPointCloud);
        static int framecount = 0;
        double timestamp = cloudHeader.stamp.toSec();
        framecount++;
        std::cout<<"frame"<<framecount<<" stamp"<<timestamp<<endl;

        std::vector<int> indices;
        pcl::removeNaNFromPointCloud(*fullPointCloud, *fullPointCloud, indices);


        int Cloudsize = fullPointCloud->points.size();
//        zvision reverse
        if(zvision_reverse) {
            for (size_t i = 0; i < Cloudsize; i++) {
                fullPointCloud->points[i].x = -fullPointCloud->points[i].x;
                fullPointCloud->points[i].z = -fullPointCloud->points[i].z;
            }
        }

//        只要20*20以内的点云
        std::vector<int> boxIndices_boundary;
        boundary_box_filter_.setInputCloud(fullPointCloud);
        boundary_box_filter_.filter(boxIndices_boundary);//
        boundary_box_filter_.filter(*fullPointCloud);

        std::vector<int> boxIndices_boundary2;
        nearby_box_filter_.setInputCloud(fullPointCloud);
        nearby_box_filter_.filter(boxIndices_boundary2);//
        nearby_box_filter_.filter(*fullPointCloud);

        if(framecount == 63){
            string filename = "/home/fjy/Desktop/PCDFILE2/1229_ece_grass2/pcd/16.pcd";
            pcl::io::savePCDFile(filename,*fullPointCloud);
        }
        if(framecount == 130){
            string filename = "/home/fjy/Desktop/PCDFILE2/1229_ece_grass2/pcd/17.pcd";
            pcl::io::savePCDFile(filename,*fullPointCloud);
        }
        if(framecount == 159){
            string filename = "/home/fjy/Desktop/PCDFILE2/1229_ece_grass2/pcd/18.pcd";
            pcl::io::savePCDFile(filename,*fullPointCloud);
        }
        if(framecount == 167){
            string filename = "/home/fjy/Desktop/PCDFILE2/1229_ece_grass2/pcd/19.pcd";
            pcl::io::savePCDFile(filename,*fullPointCloud);
        }
        if(framecount == 244){
            string filename = "/home/fjy/Desktop/PCDFILE2/1229_ece_grass2/pcd/20.pcd";
            pcl::io::savePCDFile(filename,*fullPointCloud);
        }
        if(framecount == 319){
            string filename = "/home/fjy/Desktop/PCDFILE2/1229_ece_grass2/pcd/21.pcd";
            pcl::io::savePCDFile(filename,*fullPointCloud);
        }
        if(framecount == 440){
            string filename = "/home/fjy/Desktop/PCDFILE2/1229_ece_grass2/pcd/22.pcd";
            pcl::io::savePCDFile(filename,*fullPointCloud);
        }

//        if(timestamp == 1640698596.623213){
//            std::cout<<"fuck";
//        }
        // Remove Nan points

    }

    void PublishCloud(){
        sensor_msgs::PointCloud2 laserCloudTemp;

        if (ground_pub_.getNumSubscribers() != 0) {
            pcl::toROSMsg(*ground_cloud, laserCloudTemp);
            laserCloudTemp.header.stamp = cloudHeader.stamp;
            laserCloudTemp.header.frame_id = "zvision_lidar";
            ground_pub_.publish(laserCloudTemp);
        }

        if (obstacle_pub_.getNumSubscribers() != 0) {
            pcl::toROSMsg(*cluster_cloud, laserCloudTemp);
            laserCloudTemp.header.stamp = cloudHeader.stamp;
            laserCloudTemp.header.frame_id = "zvision_lidar";
            obstacle_pub_.publish(laserCloudTemp);
        }
        if (reversed_zvision_pub_.getNumSubscribers() != 0) {
            pcl::toROSMsg(*noground_cloud, laserCloudTemp);
            laserCloudTemp.header.stamp = cloudHeader.stamp;
            laserCloudTemp.header.frame_id = "zvision_lidar";
            reversed_zvision_pub_.publish(laserCloudTemp);
        }

//        dsMethod->ComputeDrivableArea(slope_cloud,*cluster_cloud);
        if (slope_cloud_pub_.getNumSubscribers() != 0) {
            pcl::toROSMsg(*filtered_slope_cloud, laserCloudTemp);
            laserCloudTemp.header.stamp = cloudHeader.stamp;
            laserCloudTemp.header.frame_id = "zvision_lidar";
            slope_cloud_pub_.publish(laserCloudTemp);
        }
    }
};


    int main(int argc, char **argv) {
        ros::init(argc, argv, "ground_segmentation");

        ros::NodeHandle nh("~");

        // Do parameter stuff.
        GroundSegmentationParams params;
        nh.param("visualize", params.visualize, params.visualize);
        nh.param("n_bins", params.n_bins, params.n_bins);
        nh.param("n_segments", params.n_segments, params.n_segments);
        nh.param("max_dist_to_line", params.max_dist_to_line, params.max_dist_to_line);
        nh.param("max_slope", params.max_slope, params.max_slope);
        nh.param("min_slope", params.min_slope, params.min_slope);
        nh.param("long_threshold", params.long_threshold, params.long_threshold);
        nh.param("max_long_height", params.max_long_height, params.max_long_height);
        nh.param("max_start_height", params.max_start_height, params.max_start_height);
        nh.param("sensor_height", params.sensor_height, params.sensor_height);
        nh.param("line_search_angle", params.line_search_angle, params.line_search_angle);
        nh.param("n_threads", params.n_threads, params.n_threads);
        nh.param("zvision_reverse", zvision_reverse,zvision_reverse);

        // Params that need to be squared.
        double r_min, r_max, max_fit_error;
        if (nh.getParam("r_min", r_min)) {
            params.r_min_square = r_min * r_min;
        }
        if (nh.getParam("r_max", r_max)) {
            params.r_max_square = r_max * r_max;
        }
        if (nh.getParam("max_fit_error", max_fit_error)) {
            params.max_error_square = max_fit_error * max_fit_error;
        }

        std::string ground_topic, obstacle_topic, input_topic;
        bool latch;
        nh.param<std::string>("input_topic", input_topic, "/zvision_lidar_points");
        nh.param<std::string>("ground_output_topic", ground_topic, "/ground_cloud");
        nh.param<std::string>("obstacle_output_topic", obstacle_topic, "/obstacle_cloud");
        nh.param("latch", latch, false);

        fullPointCloud.reset(new pcl::PointCloud<pcl::PointXYZ>());
        ground_cloud.reset(new pcl::PointCloud<pcl::PointXYZ>());
        noground_cloud.reset(new pcl::PointCloud<pcl::PointXYZ>());
        slope_cloud.reset(new pcl::PointCloud<pcl::PointXYZ>());
        filtered_slope_cloud.reset(new pcl::PointCloud<pcl::PointXYZI>());
        cluster_cloud.reset(new pcl::PointCloud<pcl::PointXYZI>());

//        google::InitGoogleLogging(argv[0]);
//        FLAGS_log_dir = (string)WORK_SPACE_PATH + "/Log";
//        FLAGS_alsologtostderr = true;
//        FLAGS_colorlogtostderr = true;
//        FLAGS_log_prefix = true;
//        FLAGS_logbufsecs = 0;
//        FileManager::CreateDirectory(FLAGS_log_dir);

        // Start node.
        SegmentationNode node(nh, ground_topic, obstacle_topic, params, latch);
        ros::Subscriber cloud_sub;
        cloud_sub = nh.subscribe(input_topic, 1, &SegmentationNode::scanCallback, &node);


//        ros::Publisher pubcloud = nh.advertise<pcl::PointCloud<pcl::PointXYZ>>("/pcdcloud", 1, latch);
//
//        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
//        sensor_msgs::PointCloud2 cloud_blob;//PointCloud2适合版本低的点云文件
//        pcl::io::loadPCDFile("/home/fjy/Desktop/PCDFILE/1228_216_slope/1640698597.541217000.pcd",cloud_blob);
//        pcl::fromROSMsg(cloud_blob,*cloud);
//
//        sensor_msgs::PointCloud2 laserCloudTemp;
//        if (pubcloud.getNumSubscribers() != 0) {
//            pcl::toROSMsg(*filtered_slope_cloud, laserCloudTemp);
//            laserCloudTemp.header.stamp = cloudHeader.stamp;
//            laserCloudTemp.header.frame_id = "zvision_lidar";
//            pubcloud.publish(cloud_blob);
//        }

        ros::spin();
    }
